﻿#include "Object.h"
/******************************************************************************
  @Project Name  : 03TestFCLCollision
  @File Name     : Object.cpp
  @Version       : 1.0
  @Function      : to be determined
  @Author        : unishuai
  @Date          : 2024-05-20
******************************************************************************/

Object::Object(std::string name)
{
    m_Name = name;
}


RobotObj::RobotObj(std::string name, int dof, std::vector<std::shared_ptr<Model>> model, std::vector<Eigen::Matrix4d> base, std::vector<Eigen::Matrix4d> partT)
    :Object(name),
      m_Dof(dof),
      m_Model(model),
      m_Base(base),
      m_PartT_ini(partT)
{
    //这里一共有8个属性，初始化了5个，也就是说还有3个,自由度和关节的数量相同
    Eigen::VectorXd joint(this->m_Dof);
    for (auto i = 0; i < this->m_Dof; i++)
    {
        joint(i) = 0;
    }
    //更新了m_ColliObj,m_PartT和m_TransIiNow
    updateStatus(joint, 0);
}

void RobotObj::updateStatus(Eigen::VectorXd jointangle,int base_index)
{
    this->m_ColliObj.clear();
    /*
     * Eigen::VectorXd jointangle (这个是我传入的angle),int base_index(这个是我传入的index值)
     */
    calcTransIiNow(jointangle,base_index);
    this->setMColliObj();
}

void RobotObj::updateStatus(Eigen::VectorXd jointangle, Eigen::Matrix4d T_install)
{
    this->m_ColliObj.clear();
    calcTransIiNow(jointangle,T_install);
    this->setMColliObj();
}

void RobotObj::updateStatusBySliderAndAngle(Eigen::VectorXd jointSliderAndAngle, int base_index)
{
    this->m_ColliObj.clear();
    /*
     * Eigen::VectorXdjointSliderAndAngle (这个是我传入的slider和angle),int base_index(这个是我传入的index值)
     */
    calcTransIiNowBySliderAndAngle(jointSliderAndAngle,base_index);
    this->setMColliObj();
}

void RobotObj::calcTransIiNowBySliderAndAngle(Eigen::VectorXd jntSliderAndRad, int base_index)
{
    /*
     * Eigen::VectorXd jointangle (这个是我传入的angle),int base_index(这个是我传入的index值)
     */
    //base_index这个baseLink也就是指定一下，不知道有啥子用

    this->m_TransIiNow.clear();
    //更新了一下m_PartT的参数
    calcTransijNowBySliderAndAngle(jntSliderAndRad);
    //
    for (auto i = 0; i < this->m_Dof + 1; i++)
    {
        if (i == 0)
        {
            Eigen::Matrix4d transi0 = this->m_Base[base_index] * this->m_PartT[0];
            this->m_TransIiNow.push_back(transi0);
        }
        else
        {
            //m_TransIiNow是相对于世界坐标系
            //m_PartT这个是相对于上一个坐标系
            Eigen::Matrix4d trans0itemp = this->m_TransIiNow[i - 1];
            Eigen::Matrix4d transijtemp = this->m_PartT[i];
            Eigen::Matrix4d trans0inow = trans0itemp * transijtemp;
            //这个左乘右乘我就很迷，真想把运动学创始人炸了。。
            this->m_TransIiNow.push_back(trans0inow);
        }
    }
}

void RobotObj::calcTransijNowBySliderAndAngle(Eigen::VectorXd &jntSliderAndAngle)
{
    //jntRad 这个是我传入的angle
    //这个主要就是计算变换矩阵
    std::vector<Eigen::Matrix4d> transchange;
    this->m_PartT.clear();
    //把旋转的角度转化成变换矩阵
    //这里的transchange就是传回来的变换矩阵
    calcTransChangeBySliderAndAngle(jntSliderAndAngle, transchange);

    for (auto i = 0; i < this->m_Dof + 1; i++)
    {
        if (i == 0)
        {
            //不同于机械臂，我这里需要把最初始的那一个值也需要进行移动
            this->m_PartT.push_back(this->m_PartT_ini[0]*transchange[i]);
        }
        else
        {
            Eigen::Matrix4d transchangetemp = transchange[i];
            Eigen::Matrix4d transijtemp = this->m_PartT_ini[i];
            Eigen::Matrix4d transijnow = transijtemp * transchangetemp;
            this->m_PartT.push_back(transijnow);
        }
    }
}

void RobotObj::calcTransChangeBySliderAndAngle(Eigen::VectorXd &jntsliderAndAngle, std::vector<Eigen::Matrix4d> &transChange)
{
    //Eigen::VectorXd& jntRad 这个是我传入的角度值,  std::vector<Eigen::Matrix4d>& transChange 这个是新创建的变量
    Eigen::Matrix4d dtrans;

    dtrans=RobotBasic::PX(jntsliderAndAngle[0]);

    transChange.push_back(dtrans);
    for (auto i = 1; i < m_Dof+1; i++)
    {
        //这个是把我传入的角度直接控制在y轴上面
        dtrans=RobotBasic::RZ(jntsliderAndAngle[i]);

        transChange.push_back(dtrans);
    }
}


void RobotObj::calcTransIiNow(Eigen::VectorXd&jntRad, int base_index)
{
    /*
     * Eigen::VectorXd jointangle (这个是我传入的angle),int base_index(这个是我传入的index值)
     */
    //base_index这个baseLink也就是指定一下，不知道有啥子用

    this->m_TransIiNow.clear();
    //更新了一下m_PartT的参数
    calcTransijNow(jntRad);
    //
    for (auto i = 0; i < this->m_Dof + 1; i++)
    {
        if (i == 0)
        {
            Eigen::Matrix4d transi0 = this->m_Base[base_index] * this->m_PartT[0];
            this->m_TransIiNow.push_back(transi0);
        }
        else
        {
            //m_TransIiNow是相对于世界坐标系
            //m_PartT这个是相对于上一个坐标系
            Eigen::Matrix4d trans0itemp = this->m_TransIiNow[i - 1];
            Eigen::Matrix4d transijtemp = this->m_PartT[i];
            Eigen::Matrix4d trans0inow = trans0itemp * transijtemp;
            //这个左乘右乘我就很迷，真想把运动学创始人炸了。。
            this->m_TransIiNow.push_back(trans0inow);
        }
    }
}

void RobotObj::calcTransIiNow(Eigen::VectorXd &jntRad, Eigen::Matrix4d T_install)
{
    this->m_TransIiNow.clear();
    calcTransijNow(jntRad);
    for (auto i = 0; i < this->m_Dof + 1; i++)
    {
        if (i == 0)
        {
            Eigen::Matrix4d transi0 = T_install * this->m_PartT[0];
            this->m_TransIiNow.push_back(transi0);
        }
        else
        {
            Eigen::Matrix4d trans0itemp = this->m_TransIiNow[i - 1];
            Eigen::Matrix4d transijtemp = this->m_PartT[i];
            Eigen::Matrix4d trans0inow = trans0itemp * transijtemp;
            this->m_TransIiNow.push_back(trans0inow);
        }
    }
}


void RobotObj::calcTransijNow(Eigen::VectorXd& jntRad)
{
    //jntRad 这个是我传入的angle
    //这个主要就是计算变换矩阵
    std::vector<Eigen::Matrix4d> transchange;
    this->m_PartT.clear();
    //把旋转的角度转化成变换矩阵
    //这里的transchange就是传回来的变换矩阵
    calcTransChange(jntRad, transchange);

    for (auto i = 0; i < this->m_Dof + 1; i++)
    {
        if (i == 0)
        {
            this->m_PartT.push_back(this->m_PartT_ini[0]);
        }
        else
        {
            Eigen::Matrix4d transchangetemp = transchange[i-1];
            Eigen::Matrix4d transijtemp = this->m_PartT_ini[i];
            Eigen::Matrix4d transijnow = transijtemp * transchangetemp;
            this->m_PartT.push_back(transijnow);
        }
    }
}


void RobotObj::calcTransChange(Eigen::VectorXd& jntRad, std::vector<Eigen::Matrix4d>& transChange)
{
    //Eigen::VectorXd& jntRad 这个是我传入的角度值,  std::vector<Eigen::Matrix4d>& transChange 这个是新创建的变量
    Eigen::Matrix4d dtrans;
    for (auto i = 0; i < m_Dof; i++)
    {
        //这个是把我传入的角度直接控制在y轴上面
        dtrans=RobotBasic::RZ(jntRad[i]);

        transChange.push_back(dtrans);
    }
}

void RobotObj::setMColliObj()
{
    for (auto i = 0; i < this->m_Dof + 1; i++)
    {
        fcl::CollisionObjectd colliobjtemp = fcl::CollisionObjectd(this->m_Model[i], this->m_TransIiNow[i].block(0, 0, 3, 3), this->m_TransIiNow[i].block(0, 3, 3, 1));
        this->m_ColliObj.push_back(colliobjtemp);
    }
}

TunnelWallObj::TunnelWallObj(std::string name, std::shared_ptr<Model> model, Eigen::Matrix4d partT)
    :Object(name),
      m_Model(model),
      m_PartT(partT),
      m_ColliObj(model, partT.block(0, 0, 3, 3),partT.block(0, 3, 3, 1))
{}

void TunnelWallObj::updateStatus()
{
    this->setStatus(this->m_PartT);
}

void TunnelWallObj::updateStatus(double angle)
{
    Eigen::Matrix4d dtrans=RobotBasic::RZ(angle);
    this->setStatus(this->m_PartT*dtrans);
}

void TunnelWallObj::setStatus(Eigen::Matrix4d trans)
{
    this->m_ColliObj.setRotation(trans.block(0, 0, 3, 3));
    this->m_ColliObj.setTranslation(trans.block(0, 3, 3, 1));
}

void TunnelWallObj::setTransformMatrixByPosAndRotVector(std::array<double, 6> pose)
{
    Eigen::Matrix4d newParT=RobotBasic::PE2T(pose[0],pose[1],pose[2],pose[3],pose[4],pose[5]);
    this->m_PartT=newParT;
    this->setStatus(this->m_PartT);
}

Eigen::Matrix4d TunnelWallObj::getTransformMatrix() const
{
    return m_PartT;
}

void TunnelWallObj::setTransformMatrix(const Eigen::Matrix4d &PartT)
{
    m_PartT = PartT;
    this->setStatus(this->m_PartT);
}




RadarObj::RadarObj(std::string name, int dof, std::vector<std::shared_ptr<Model>> model, std::vector<Eigen::Matrix4d> trans)
    :Object (name),
      m_dof(dof),
      m_Model(model),
      m_IniRelTrans(trans)
{
    Eigen::VectorXd angle(this->m_dof);
    for(int i=0; i<this->m_dof; i++)
    {
        angle[i]=0;
    }
    updateStatus(angle);
}

void RadarObj::updateStatus(Eigen::VectorXd &jntRad)
{
    calcAbsTrans(jntRad);
    setMColliObj();
}

//这个就是获取到了经历过旋转的，每一个模型相对于世界坐标的举证
void RadarObj::calcAbsTrans(Eigen::VectorXd &jntRad)
{
    this->m_NowAbsTrans.clear();
    std::vector<Eigen::Matrix4d> transChange;
    for(int i=0;i<m_dof;i++)
    {
        transChange.push_back(this->m_IniRelTrans[i]*RobotBasic::RZ(jntRad[i]));
    }

    for(int i=0;i<this->m_dof;i++)
    {
        if(i==0)
        {
            this->m_NowAbsTrans.push_back(transChange[0]);
        }
        else
        {
            this->m_NowAbsTrans.push_back(this->m_NowAbsTrans[i-1]*transChange[i]);
        }
    }
}

void RadarObj::setMColliObj()
{
    this->m_ColliObj.clear();
    for (auto i = 0; i < this->m_dof; i++)
    {
        //这个block相当于是加了一个掩码,然后取出了它的子矩阵
        fcl::CollisionObjectd colliobjtemp = fcl::CollisionObjectd(this->m_Model[i], this->m_NowAbsTrans[i].block(0, 0, 3, 3), this->m_NowAbsTrans[i].block(0, 3, 3, 1));
        this->m_ColliObj.push_back(colliobjtemp);
    }
}

FlightObj::FlightObj(std::string name, int dof, std::vector<std::shared_ptr<Model>> model, std::vector<Eigen::Matrix4d> base, std::vector<Eigen::Matrix4d> partT)
    :Object(name),
     m_Dof(dof),
     m_Model(model),
     m_Base(base),
     m_PartT_ini(partT)
{
    //这里一共有8个属性，初始化了5个，也就是说还有3个,自由度和关节的数量相同
    Eigen::VectorXd joint(this->m_Dof);
    for (auto i = 0; i < this->m_Dof; i++)
    {
        joint(i) = 0;
    }
    //更新了m_ColliObj,m_PartT和m_TransIiNow
    updateStatus(joint, 0);
}


void FlightObj::updateStatus(Eigen::VectorXd jointangle,int base_index)
{
    this->m_ColliObj.clear();
    /*
     * Eigen::VectorXd jointangle (这个是我传入的angle),int base_index(这个是我传入的index值)
     */
    calcTransIiNow(jointangle,base_index);
    this->setMColliObj();
}


void FlightObj::updateStatus(Eigen::VectorXd jointangle, Eigen::Matrix4d T_install)
{
    this->m_ColliObj.clear();
    calcTransIiNow(jointangle,T_install);
    this->setMColliObj();
}


void FlightObj::calcTransIiNow(Eigen::VectorXd&jntRad, int base_index)
{
    /*
     * Eigen::VectorXd jointangle (这个是我传入的angle),int base_index(这个是我传入的index值)
     */
    //base_index这个baseLink也就是指定一下，不知道有啥子用

    this->m_TransIiNow.clear();
    //更新了一下m_PartT的参数
    calcTransijNow(jntRad);
    //
    for (auto i = 0; i < this->m_Dof + 1; i++)
    {
        if (i == 0)
        {
            Eigen::Matrix4d transi0 = this->m_Base[base_index] * this->m_PartT[0];
            this->m_TransIiNow.push_back(transi0);
        }
        else
        {
            //m_TransIiNow是相对于世界坐标系
            //m_PartT这个是相对于上一个坐标系
            Eigen::Matrix4d trans0itemp = this->m_TransIiNow[i - 1];
            Eigen::Matrix4d transijtemp = this->m_PartT[i];
            Eigen::Matrix4d trans0inow = trans0itemp * transijtemp;
            //这个左乘右乘我就很迷，真想把运动学创始人炸了。。
            this->m_TransIiNow.push_back(trans0inow);
        }
    }
}

void FlightObj::calcTransIiNow(Eigen::VectorXd &jntRad, Eigen::Matrix4d T_install)
{
    this->m_TransIiNow.clear();
    calcTransijNow(jntRad);
    for (auto i = 0; i < this->m_Dof + 1; i++)
    {
        if (i == 0)
        {
            Eigen::Matrix4d transi0 = T_install * this->m_PartT[0];
            this->m_TransIiNow.push_back(transi0);
        }
        else
        {
            Eigen::Matrix4d trans0itemp = this->m_TransIiNow[i - 1];
            Eigen::Matrix4d transijtemp = this->m_PartT[i];
            Eigen::Matrix4d trans0inow = trans0itemp * transijtemp;
            this->m_TransIiNow.push_back(trans0inow);
        }
    }
}

void FlightObj::calcTransijNow(Eigen::VectorXd& jntRad)
{
    //jntRad 这个是我传入的angle
    //这个主要就是计算变换矩阵
    std::vector<Eigen::Matrix4d> transchange;
    this->m_PartT.clear();
    //把旋转的角度转化成变换矩阵
    //这里的transchange就是传回来的变换矩阵
    calcTransChange(jntRad, transchange);

    for (auto i = 0; i < this->m_Dof + 1; i++)
    {
        if (i == 0)
        {
            this->m_PartT.push_back(this->m_PartT_ini[0]*transchange[0]);
        }
        else
        {
            Eigen::Matrix4d transchangetemp = transchange[i];
            Eigen::Matrix4d transijtemp = this->m_PartT_ini[i];
            Eigen::Matrix4d transijnow = transijtemp * transchangetemp;
            this->m_PartT.push_back(transijnow);
        }
    }
}

void FlightObj::calcTransChange(Eigen::VectorXd& jntRad, std::vector<Eigen::Matrix4d>& transChange)
{
    //Eigen::VectorXd& jntRad 这个是我传入的角度值,  std::vector<Eigen::Matrix4d>& transChange 这个是新创建的变量
    Eigen::Matrix4d dtrans;
    for (auto i = 0; i < m_Dof+1; i++)
    {
        //这个是把我传入的角度直接控制在y轴上面
        dtrans=RobotBasic::RZ(jntRad[i]);

        transChange.push_back(dtrans);
    }
}

void FlightObj::setMColliObj()
{
    for (auto i = 0; i < this->m_Dof + 1; i++)
    {
        fcl::CollisionObjectd colliobjtemp = fcl::CollisionObjectd(this->m_Model[i], this->m_TransIiNow[i].block(0, 0, 3, 3), this->m_TransIiNow[i].block(0, 3, 3, 1));
        this->m_ColliObj.push_back(colliobjtemp);
    }
}





MotionPlatformObj::MotionPlatformObj(std::string name, int dof, std::vector<std::shared_ptr<Model>> model, std::vector<Eigen::Matrix4d> base, std::vector<Eigen::Matrix4d> partT)
    :Object(name),
      m_Dof(dof),
      m_Model(model),
      m_Base(base),
      m_PartT_ini(partT)
{
    //这里一共有8个属性，初始化了5个，也就是说还有3个,自由度和关节的数量相同
    Eigen::VectorXd joint(this->m_Dof);
    for (auto i = 0; i < this->m_Dof; i++)
    {
        joint(i) = 0;
    }
    //更新了m_ColliObj,m_PartT和m_TransIiNow
    //    updateStatus(joint, 0);
    Eigen::Matrix4d globalT=base[1];
    // 提取旋转矩阵部分
    Eigen::Matrix3d R = globalT.block<3,3>(0,0);
    // 提取平移向量部分
    Eigen::Vector3d p = globalT.block<3,1>(0,3);
    // 将旋转矩阵转换为欧拉角，这里使用的旋转顺序为 ZYX
    Eigen::Vector3d euler = R.eulerAngles(2, 1, 0);
    Eigen::VectorXd globalVX(6);

    globalVX<<p[0],p[1],p[2],euler[2],euler[1],euler[0];
//   std::cout<<"-----------"<<std::endl;
    updateStatusByDofAndRot(globalVX,joint,0);
}

void MotionPlatformObj::updateStatus(Eigen::VectorXd jointangle, int base_index)
{
    //    this->m_ColliObj.clear();
    //    /*
    //     * Eigen::VectorXd jointangle (这个是我传入的angle),int base_index(这个是我传入的index值)
    //     */
    //    calcTransIiNow(jointangle,base_index);
    //    this->setMColliObj();
    std::cout<<"MotionPlatformObj::updateStatus(Eigen::VectorXd jointangle, int base_index) is discarded"<<std::endl;
}

void MotionPlatformObj::updateStatus(Eigen::VectorXd jointangle, Eigen::Matrix4d T_install)
{
    this->m_ColliObj.clear();
    calcTransIiNow(jointangle,T_install);
    this->setMColliObj();
}

void MotionPlatformObj::updateStatusByDofAndRot(Eigen::VectorXd baseDof, Eigen::VectorXd jointangle, int base_index)
{
    this->m_ColliObj.clear();
    /*
     * Eigen::VectorXd jointangle (这个是我传入的angle),int base_index(这个是我传入的index值)
     */
    calcTransIiNow(baseDof,jointangle,base_index);
    this->setMColliObj();
}

void MotionPlatformObj::calcTransIiNow(Eigen::VectorXd &baseDof,Eigen::VectorXd &jntRad, int base_index)
{
    /*
     * Eigen::VectorXd jointangle (这个是我传入的angle),int base_index(这个是我传入的index值)
     */
    //base_index这个baseLink也就是指定一下，不知道有啥子用

    this->m_TransIiNow.clear();
    //更新了一下m_PartT的参数
    calcTransijNow(baseDof,jntRad);
    //
    for (auto i = 0; i < this->m_Dof + 1; i++)
    {
        if (i == 0)
        {
            Eigen::Matrix4d transi0 = this->m_Base[base_index] * this->m_PartT[0];
            this->m_TransIiNow.push_back(transi0);
        }
        else
        {
            //m_TransIiNow是相对于世界坐标系
            //m_PartT这个是相对于上一个坐标系
            Eigen::Matrix4d trans0itemp = this->m_TransIiNow[i - 1];
            Eigen::Matrix4d transijtemp = this->m_PartT[i];
            Eigen::Matrix4d trans0inow = trans0itemp * transijtemp;
            //这个左乘右乘我就很迷，真想把运动学创始人炸了。。
            this->m_TransIiNow.push_back(trans0inow);
        }
    }
}

void MotionPlatformObj::calcTransIiNow(Eigen::VectorXd &jntRad, Eigen::Matrix4d T_install)
{
    //    this->m_TransIiNow.clear();
    //    calcTransijNow(jntRad);
    //    for (auto i = 0; i < this->m_Dof + 1; i++)
    //    {
    //        if (i == 0)
    //        {
    //            Eigen::Matrix4d transi0 = T_install * this->m_PartT[0];
    //            this->m_TransIiNow.push_back(transi0);
    //        }
    //        else
    //        {
    //            Eigen::Matrix4d trans0itemp = this->m_TransIiNow[i - 1];
    //            Eigen::Matrix4d transijtemp = this->m_PartT[i];
    //            Eigen::Matrix4d trans0inow = trans0itemp * transijtemp;
    //            this->m_TransIiNow.push_back(trans0inow);
    //        }
    //    }
    std::cout<<" this function is discarded"<<std::endl;
}

void MotionPlatformObj::calcTransijNow(Eigen::VectorXd &baseDof,Eigen::VectorXd &jntRad)
{
    //jntRad 这个是我传入的angle
    //这个主要就是计算变换矩阵
    std::vector<Eigen::Matrix4d> transchange;
    this->m_PartT.clear();
    //把旋转的角度转化成变换矩阵
    //这里的transchange就是传回来的变换矩阵
    calcTransChange(baseDof,jntRad, transchange);

    for (auto i = 0; i < this->m_Dof + 1; i++)
    {
        if (i == 0)
        {
//            this->m_PartT.push_back(this->m_PartT_ini[0]*transchange[0]);
            this->m_PartT.push_back(transchange[0]);
        }
        else
        {
            Eigen::Matrix4d transchangetemp = transchange[i];
            Eigen::Matrix4d transijtemp = this->m_PartT_ini[i];
            Eigen::Matrix4d transijnow = transijtemp * transchangetemp;
            this->m_PartT.push_back(transijnow);
        }
    }
}

void MotionPlatformObj::calcTransChange(Eigen::VectorXd &baseDof,Eigen::VectorXd &jntRad, std::vector<Eigen::Matrix4d> &transChange)
{
    //Eigen::VectorXd& jntRad 这个是我传入的角度值,  std::vector<Eigen::Matrix4d>& transChange 这个是新创建的变量
    Eigen::Matrix4d dtrans;
    dtrans=RobotBasic::PE2T(baseDof[0],baseDof[1],baseDof[2],baseDof[3],baseDof[4],baseDof[5]);
    transChange.push_back(dtrans);

    for (auto i = 0; i < m_Dof; i++)
    {
        //这个是把我传入的角度直接控制在y轴上面
        dtrans=RobotBasic::RZ(jntRad[i]);

        transChange.push_back(dtrans);
    }
}

void MotionPlatformObj::setMColliObj()
{
    for (auto i = 0; i < this->m_Dof + 1; i++)
    {
        fcl::CollisionObjectd colliobjtemp = fcl::CollisionObjectd(this->m_Model[i], this->m_TransIiNow[i].block(0, 0, 3, 3), this->m_TransIiNow[i].block(0, 3, 3, 1));
        this->m_ColliObj.push_back(colliobjtemp);
    }
}
